#pragma once
#include "Vector.h"


template <typename T>
struct Matrix4;

template <typename T>
struct Matrix3;


template <typename T>
struct QuaternionT {
	T x;
	T y;
	T z;
	T w;

	QuaternionT();
	QuaternionT(T x, T y, T z, T w);

	QuaternionT<T> Slerp(T mu, const QuaternionT<T>& q) const;
	QuaternionT<T> Rotated(const QuaternionT<T>& b) const;
	QuaternionT<T> Scaled(T scale) const;
	T Dot(const QuaternionT<T>& q) const;
	Vector4<T> ToVector() const;
	QuaternionT<T> operator-(const QuaternionT<T>& q) const;
	QuaternionT<T> operator+(const QuaternionT<T>& q) const;
	Vector3<T>     operator*(const Vector3<T>&     v) const;

	const QuaternionT<T> operator*(const QuaternionT<T>& rhs) const;
	QuaternionT<T> operator /= (float rhs);
	QuaternionT<T> operator / (float rhs) const;
	
	bool operator==(const QuaternionT<T>& q) const;
	bool operator!=(const QuaternionT<T>& q) const;

	void Normalize();
	void SetIdentity();
	void Rotate(const QuaternionT<T>& q);

	static QuaternionT<T> CreateFromVectors(const Vector3<T>& v0, const Vector3<T>& v1);
	static QuaternionT<T> CreateFromAxisAngle(const Vector3<T>& axis, float radians);
    static QuaternionT<T> CreateRotationVDir( const Vector3<T>& vdir );
    static QuaternionT<T> QuatBetweenVectors(const Vector3<T>& v0, const Vector3<T>& v1);
    

	void SetRotationVDir( const Vector3<T>& vdir );
		
	void SetRotate( float  angle, T  _x , T  _y , T  _z );
	void SetRotate ( float  angle, const Vector3<T>& vec );

	void SetRotate ( float  angle1, const Vector3<T>& axis1, 
		              float  angle2, const Vector3<T>& axis2,
	                  float  angle3, const Vector3<T>& axis3);

	float Length()  const;
	float Length2() const;	

	QuaternionT<T> Conjugate() const;
	const QuaternionT<T> Inverse() const;
};


template <typename T>
inline QuaternionT<T>::QuaternionT() : x(0), y(0), z(0), w(1)
{ 
}

template <typename T>
inline QuaternionT<T>::QuaternionT(T x, T y, T z, T w) : x(x), y(y), z(z), w(w)
{
}

template <typename T>
inline void QuaternionT<T>::SetIdentity( void )
{  
   x = 0.f; 
   y = 0.f;
   z = 0.f;
   w = 1.f; 
}

// Ken Shoemake's famous method.
template <typename T>
inline QuaternionT<T> QuaternionT<T>::Slerp(T t, const QuaternionT<T>& v1) const
{
	const T epsilon = 0.0005f;
	T dot = Dot(v1);

	if (dot > 1 - epsilon) {
		QuaternionT<T> result = v1 + (*this - v1).Scaled(t);
		result.Normalize();
		return result;
	}

	if (dot < 0)
		dot = 0;

	if (dot > 1)
		dot = 1;

	T theta0 = std::acos(dot);
	T theta = theta0 * t;

	QuaternionT<T> v2 = (v1 - Scaled(dot));
	v2.Normalize();

	QuaternionT<T> q = Scaled(std::cos(theta)) + v2.Scaled(std::sin(theta));
	q.Normalize();
	return q;
}

template <typename T>
inline QuaternionT<T> QuaternionT<T>::Rotated(const QuaternionT<T>& b) const
{
	QuaternionT<T> q;
	q.w = w * b.w - x * b.x - y * b.y - z * b.z;
	q.x = w * b.x + x * b.w + y * b.z - z * b.y;
	q.y = w * b.y + y * b.w + z * b.x - x * b.z;
	q.z = w * b.z + z * b.w + x * b.y - y * b.x;
	q.Normalize();
	return q;
}

template <typename T>
inline void QuaternionT<T>::SetRotationVDir( const Vector3<T>& vdir )
{
	if(vdir.Length() <= 0.0f)
	{ return; 
	}
		
	//set default initialisation for up-vector	
	w= T(0.70710676908493042);	x = T((vdir.z+vdir.z)*0.35355338454246521);	y=T(0.0); 	z=T(0.0); 
	T l = sqrt(vdir.x*vdir.x+vdir.y*vdir.y);
	if (l>0.00001)	
	{
		//calculate LookAt quaternion
		Vector3<T> hv	=	Vector3<T>(vdir.x/l,vdir.y/l+1.0f,l+1.0f);
		T r   = sqrt(hv.x*hv.x + hv.y*hv.y);
	    T s   = sqrt(hv.z*hv.z + vdir.z*vdir.z);
		//generate the half-angle sine&cosine
		T hacos0=0.0;			T hasin0=-1.0;			
		if (r>0.00001) { hacos0=hv.y/r;  hasin0=-hv.x/r; }	    //yaw
		T hacos1=hv.z/s;	T hasin1=vdir.z/s;					//pitch
		w=T(hacos0*hacos1); x=T(hacos0*hasin1);	y=T(hasin0*hasin1);	z=T(hasin0*hacos1);  
	}
}

template <typename T>
QuaternionT<T> QuaternionT<T>::CreateRotationVDir( const Vector3<T>& vdir ) 
{
   QuaternionT<T> q;
   q.SetRotationVDir(vdir);
   return q;
}


template <typename T>
inline QuaternionT<T> QuaternionT<T>::Scaled(T s) const
{
	return QuaternionT<T>(x * s, y * s, z * s, w * s);
}

template <typename T>
inline T QuaternionT<T>::Dot(const QuaternionT<T>& q) const
{
	return x * q.x + y * q.y + z * q.z + w * q.w;
}


template <typename T>
inline Vector4<T> QuaternionT<T>::ToVector() const
{
	return Vector4<T>(x, y, z, w);
}

template <typename T>
QuaternionT<T> QuaternionT<T>::operator-(const QuaternionT<T>& q) const
{
	return QuaternionT<T>(x - q.x, y - q.y, z - q.z, w - q.w);
}

template <typename T>
QuaternionT<T> QuaternionT<T>::operator+(const QuaternionT<T>& q) const
{
	return QuaternionT<T>(x + q.x, y + q.y, z + q.z, w + q.w);
}

template <typename T>
bool QuaternionT<T>::operator==(const QuaternionT<T>& q) const
{
	return x == q.x && y == q.y && z == q.z && w == q.w;
}

template <typename T>
bool QuaternionT<T>::operator!=(const QuaternionT<T>& q) const
{
	return !(*this == q);
}

template <typename T>
Vector3<T>  QuaternionT<T>::operator*(const Vector3<T>&     v) const
{
	Vector3<T>  uv, uuv; 
	Vector3<T>  qvec(x, y, z);
	uv = qvec.Cross(v);
	uuv = qvec.Cross(uv); 
	uv   = uv  * ( 2.0f * w ); 
	uuv  = uuv * 2.0f; 
	return v + uv + uuv;	
}

/// Binary multiply 
//*
template <typename T>
inline const QuaternionT<T> QuaternionT<T>::operator*(const QuaternionT<T>& rhs) const
{
	return QuaternionT<T>( rhs.w*x + rhs.x*w + rhs.y*z - rhs.z*y,
		rhs.w*y - rhs.x*z + rhs.y*w + rhs.z*x,
		rhs.w*z + rhs.x*y - rhs.y*x + rhs.z*w,
		rhs.w*w - rhs.x*x - rhs.y*y - rhs.z*z );
}
/**/

/*
template <typename T>
inline const QuaternionT<T> QuaternionT<T>::operator*(const QuaternionT<T>& rhs) const
{
	return QuaternionT<T>
		( 
				w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z ,
				w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y ,
				w * rhs.y + y * rhs.w + z * rhs.x - x * rhs.z ,
				w * rhs.z + z * rhs.w + x * rhs.y - y * rhs.x
		);
}
/**/
//Quaternion Quaternion::operator*( const Quaternion& q ) const
//{
//	return Quaternion
//		(
//		w * q.w - x * q.x - y * q.y - z * q.z ,
//		w * q.x + x * q.w + y * q.z - z * q.y ,
//		w * q.y + y * q.w + z * q.x - x * q.z ,
//		w * q.z + z * q.w + x * q.y - y * q.x
//		);
//}

/// Unary divide by scalar 
template <typename T>
QuaternionT<T> QuaternionT<T>::operator /= (float rhs)
{
	float div = 1.0f/rhs;
	x*=div;
	y*=div;
	z*=div;
	w*=div;
	return *this;
}

template <typename T>
inline QuaternionT<T> QuaternionT<T>::operator / (float rhs) const
{

	float div = 1.0f/rhs;
	return QuaternionT<T>(x*div, y*div, z*div, w*div);
}


/// Length of the quaternion = sqrt( vec . vec )
template <typename T>
float QuaternionT<T>::Length() const
{
	return sqrt( x*x + y*y + z*z + w*w);
}

/// Length of the quaternion = vec . vec
template <typename T>
float QuaternionT<T>::Length2() const
{
	return x*x + y*y + z*z + w*w;
}

/// Conjugate 
template <typename T>
inline QuaternionT<T> QuaternionT<T>::Conjugate () const
{ 
	return QuaternionT<T>( -x, -y, -z, w );
}

/// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
template <typename T>
inline const QuaternionT<T> QuaternionT<T>::Inverse () const
{
	return Conjugate() / Length2();
}

#ifdef __DX_TEST__
#include <d3dx9.h>
#endif

template <typename T>
inline QuaternionT<T> QuaternionT<T>::QuatBetweenVectors(const Vector3<T>& v0, const Vector3<T>& v1)
{
	vec3 vout;

	QuaternionT<T> q;

	vout = v0.Cross(v1);
    vout.Normalize();

	float fAngle = vec3::AngleBetweenVectors(v0,v1);

	q.SetRotate(fAngle,vout);

#ifdef __DX_TEST__

	D3DXVECTOR3     vout2;
	D3DXQUATERNION     _q; 

	vout2 = D3DXVECTOR3(vout.x,vout.y,vout.z);

    D3DXQuaternionRotationAxis(&_q,&vout2,fAngle);

	q.x = _q.x;
	q.y = _q.y;
	q.z = _q.z;
	q.w = _q.w;

#endif

	return q;
}	 



// Compute the quaternion that rotates from a to b, avoiding numerical instability.
// Taken from "The Shortest Arc Quaternion" by Stan Melax in "Game Programming Gems".
template <typename T>
inline QuaternionT<T> QuaternionT<T>::CreateFromVectors(const Vector3<T>& v0, const Vector3<T>& v1)
{
	if (v0 == -v1)
		return QuaternionT<T>::CreateFromAxisAngle(vec3(1, 0, 0), Pi);

	Vector3<T> c = v0.Cross(v1);
	T d = v0.Dot(v1);
	T s = std::sqrt((1 + d) * 2);

	QuaternionT<T> q;
	q.x = c.x / s;
	q.y = c.y / s;
	q.z = c.z / s;
	q.w = s / 2.0f;
	return q;
}

template <typename T>
inline QuaternionT<T>  QuaternionT<T>::CreateFromAxisAngle(const Vector3<T>& axis, float radians)
{
	QuaternionT<T> q;
	q.w = std::cos(radians / 2);
	q.x = q.y = q.z = std::sin(radians / 2);
	q.x *= axis.x;
	q.y *= axis.y;
	q.z *= axis.z;
	return q;
}

template <typename T>
inline void QuaternionT<T>::Normalize()
{
	*this = Scaled(1 / std::sqrt(Dot(*this)));
}


template <typename T>
inline void QuaternionT<T>::Rotate(const QuaternionT<T>& q2)
{
	QuaternionT<T> q;
	QuaternionT<T>& q1 = *this;

	q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
	q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
	q.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
	q.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;

	q.Normalize();
	*this = q;
}


template <typename T>
inline void QuaternionT<T>::SetRotate( float  angle, T  _x, T  _y, T  _z )
{

	const float epsilon = 0.0000001f;

	float length = sqrtf( _x*_x + _y*_y + _z*_z );

	if (length < epsilon)
	{
		*this = QuaternionT();

		// ~zero length axis, so reset rotation to zero.
		return;
	}

	float inversenorm  = 1.0f/length;
	float coshalfangle = cosf( 0.5f * angle );
	float sinhalfangle = sinf( 0.5f * angle );

	x = _x * sinhalfangle * inversenorm;
	y = _y * sinhalfangle * inversenorm;
	z = _z * sinhalfangle * inversenorm;
	w = coshalfangle;
}
 
//////////////////////////////////////////////////////////////////////////
template <typename T>
inline void QuaternionT<T>::SetRotate ( float  angle, const Vector3<T>& vec )
{
   SetRotate( angle, vec.x, vec.y, vec.z );
}

//////////////////////////////////////////////////////////////////////////
template <typename T>
inline void QuaternionT<T>::SetRotate ( float  angle1, const Vector3<T>& axis1, 
                                         float  angle2, const Vector3<T>& axis2,
                                         float  angle3, const Vector3<T>& axis3)
{
	QuaternionT q1; 
	q1.SetRotate(angle1,axis1);
	QuaternionT q2; 
	q2.SetRotate(angle2,axis2);
	QuaternionT q3; 
	q3.SetRotate(angle3,axis3);

	*this = q1*q2*q3;
}


typedef QuaternionT<float> Quaternion;
